#include "Camera.h"

using namespace std;

const float Camera::PI = atan(1.0)*4;

const Vec3f worldXaxis(1, 0, 0);
const Vec3f worldYaxis(0, 1, 0);
const Vec3f worldZaxis(0, 0, 1);

const GLfloat im[16] = {1,0,0,0 ,0,1,0,0, 0,0,1,0, 0,0,0,1}; // identity matrix

Camera::Camera(void)
{
	cm = new GLfloat[16];
	loadIdentity(cm);

	xaxis = new Vec3f(1,0,0);
	yaxis = new Vec3f(0,1,0);
	zaxis = new Vec3f(0,0,1);

	eyeP = new Vec3f(0,0,0);
	centerP = new Vec3f(0,0,-1);
}


Camera::~Camera(void)
{
	delete cm;
	delete xaxis;
	delete yaxis;
	delete zaxis;
	delete eyeP;
	delete centerP;
}

// WORKING
void Camera::update() const
{
	glLoadMatrixf(cm);
}

// WORKING
void Camera::pitch(const GLfloat amountInDegree)
{
	GLfloat angle = amountInDegree * PI / 180;

	GLfloat pitchMatrix[16] = {1,0,0,0, 0,cos(angle),sin(angle),0, 0,-sin(angle),cos(angle),0, 0,0,0,1};

	matrix4by4TimesMatrix4by4(pitchMatrix, cm, cm);

	setAxisFromCameraMatrix();
}

// WORKING
void Camera::pitchUp(const GLfloat amountInDegree)
{
	pitch(-amountInDegree);
}

// WORKING
void Camera::pitchDown(const GLfloat amountInDegree)
{
	pitch(amountInDegree);
}

// WORKING
void Camera::yaw(const GLfloat amountInDegree)
{
	GLfloat angle = amountInDegree * PI / 180;

	GLfloat pitchMatrix[16] = {cos(angle),0,-sin(angle),0, 0,1,0,0, sin(angle),0,cos(angle),0, 0,0,0,1};

	matrix4by4TimesMatrix4by4(pitchMatrix, cm, cm);

	setAxisFromCameraMatrix();
}

// WORKING
void Camera::yawLeft(const GLfloat amountInDegree)
{
	yaw(-amountInDegree);
}

// WORKING
void Camera::yawRight(const GLfloat amountInDegree)
{
	yaw(amountInDegree);
}

// WORKING
void Camera::roll(const GLfloat angleInDegree)
{ 
	// roll the camera through angle degrees
	GLfloat cs = cos(angleInDegree * PI / 180);
	GLfloat sn = sin(angleInDegree * PI / 180);

	Vec3f temp = *xaxis; // remember old xaxis
	*xaxis = Vec3f(cs*temp[0] - sn* (*yaxis)[0], cs*temp[1] - sn* (*yaxis)[1], cs*temp[2] - sn* (*yaxis)[2]);
	*yaxis = Vec3f(sn*temp[0] + cs* (*yaxis)[0], sn*temp[1] + cs* (*yaxis)[1], sn*temp[2] + cs* (*yaxis)[2]);

	setCameraMatrix();
}

// WORKING
void Camera::rollLeft(const GLfloat amountInDegree)
{
	roll(-amountInDegree);
}

// WORKING
void Camera::rollRight(const GLfloat amountInDegree)
{
	roll(amountInDegree);
}

// WORKING
void Camera::moveTo(const GLfloat x, const GLfloat y, const GLfloat z)
{
	(*eyeP)[0] = x;
	(*eyeP)[1] = y;
	(*eyeP)[2] = z;

	setAxisFromEyeAndCenterPoint();
	setCameraMatrix();
}

// WORKING
void Camera::lookAt(const GLfloat x, const GLfloat y, const GLfloat z)
{
	(*centerP)[0] = x;
	(*centerP)[1] = y;
	(*centerP)[2] = z;

	setAxisFromEyeAndCenterPoint();
	setCameraMatrix();
}

// WORKING
void Camera::lookAt(const GLfloat eyex, const GLfloat eyey, const GLfloat eyez, const GLfloat centerx, const GLfloat centery, const GLfloat centerz)
{
	(*eyeP) = Vec3f(eyex, eyey, eyez);
	(*centerP) = Vec3f(centerx, centery, centerz);

	setAxisFromEyeAndCenterPoint();
	setCameraMatrix();
}

// WORKING
void Camera::moveForward(const GLfloat distance)
{
	slide(0,0,-distance);
}

// WORKING
void Camera::moveBackward(const GLfloat distance) 
{
	slide(0,0,distance);
}

void Camera::moveLeft(const GLfloat distance)
{
	slide(-distance,0,0);
}

void Camera::moveRight(const GLfloat distance)
{
	slide(distance,0,0);
}

void Camera::moveUp(const GLfloat distance)
{
	slide(0,distance,0);
}

void Camera::moveDown(const GLfloat distance)
{
	slide(0,-distance,0);
}

// WORKING
void Camera::slide(const GLfloat xAmount, const GLfloat yAmount, const GLfloat zAmount)
{
	eyeP[0] += xAmount*xaxis[0] + yAmount*yaxis[0] + zAmount*zaxis[0];
	eyeP[1] += xAmount*xaxis[1] + yAmount*yaxis[1] + zAmount*zaxis[1];
	eyeP[2] += xAmount*xaxis[2] + yAmount*yaxis[2] + zAmount*zaxis[2];
	
	setCameraMatrix();
}

void Camera::rotatePointAboutAnArbitraryAxis(const GLfloat angleInDegree, const Vec3f & _axis, const Vec3f & point, Vec3f & ret) const
{
	GLfloat theta = angleInDegree * PI / 180;

	GLfloat costheta;
	GLfloat sintheta;

	Vec3f axis = _axis.normalize();

	costheta = cosf(theta);
	sintheta = sinf(theta);

	ret[0] += (costheta + (1 - costheta) * axis[0] * axis[0]) * point[0];
	ret[0] += ((1 - costheta) * axis[0] * axis[1] - axis[2] * sintheta) * point[1];
	ret[0] += ((1 - costheta) * axis[0] * axis[2] + axis[1] * sintheta) * point[2];

	ret[1] += ((1 - costheta) * axis[0] * axis[1] + axis[2] * sintheta) * point[0];
	ret[1] += (costheta + (1 - costheta) * axis[1] * axis[1]) * point[1];
	ret[1] += ((1 - costheta) * axis[1] * axis[2] - axis[0] * sintheta) * point[2];

	ret[2] += ((1 - costheta) * axis[0] * axis[2] - axis[1] * sintheta) * point[0];
	ret[2] += ((1 - costheta) * axis[1] * axis[2] + axis[0] * sintheta) * point[1];
	ret[2] += (costheta + (1 - costheta) * axis[2] * axis[2]) * point[2];
}

// WORKING
void Camera::matrix4by4TimesMatrix4by4(const GLfloat * m, const GLfloat * t, GLfloat * resultMatrix) const
{
	GLfloat tempMatrix[16];

	tempMatrix[0] = m[0]*t[0] + m[4]*t[1] + m[8]*t[2] + m[12]*t[3];
	tempMatrix[1] = m[1]*t[0] + m[5]*t[1] + m[9]*t[2] + m[13]*t[3];
	tempMatrix[2] = m[2]*t[0] + m[6]*t[1] + m[10]*t[2] + m[14]*t[3];
	tempMatrix[3] = m[3]*t[0] + m[7]*t[1] + m[11]*t[2] + m[15]*t[3];

	tempMatrix[4] = m[0]*t[4] + m[4]*t[5] + m[8]*t[6] + m[12]*t[7];
	tempMatrix[5] = m[1]*t[4] + m[5]*t[5] + m[9]*t[6] + m[13]*t[7];
	tempMatrix[6] = m[2]*t[4] + m[6]*t[5] + m[10]*t[6] + m[14]*t[7];
	tempMatrix[7] = m[3]*t[4] + m[7]*t[5] + m[11]*t[6] + m[15]*t[7];

	tempMatrix[8] = m[0]*t[8] + m[4]*t[9] + m[8]*t[10] + m[12]*t[11];
	tempMatrix[9] = m[1]*t[8] + m[5]*t[9] + m[9]*t[10] + m[13]*t[11];
	tempMatrix[10] = m[2]*t[8] + m[6]*t[9] + m[10]*t[10] + m[14]*t[11];
	tempMatrix[11] = m[3]*t[8] + m[7]*t[9] + m[11]*t[10] + m[15]*t[11];

	tempMatrix[12] = m[0]*t[12] + m[4]*t[13] + m[8]*t[14] + m[12]*t[15];
	tempMatrix[13] = m[1]*t[12] + m[5]*t[13] + m[9]*t[14] + m[13]*t[15];
	tempMatrix[14] = m[2]*t[12] + m[6]*t[13] + m[10]*t[14] + m[14]*t[15];
	tempMatrix[15] = m[3]*t[12] + m[7]*t[13] + m[11]*t[14] + m[15]*t[15];

	memcpy(resultMatrix, tempMatrix, 16*sizeof(GLfloat));
	/*
	for(unsigned int i = 0; i != 16; ++i)
	{
		resultMatrix[i] = tempMatrix[i];
	}
	*/
}

void Camera::matrix4by4TimesVector(const GLfloat * m, const Vec3f & v, Vec3f & ret) const
{
	ret[0] = v[0]*m[0] + v[1]*m[4] + v[2]*m[8] + m[12];
	ret[1] = v[0]*m[1] + v[1]*m[5] + v[2]*m[9] + m[13];
	ret[2] = v[0]*m[2] + v[1]*m[6] + v[2]*m[10] + m[14];
	GLfloat w = v[0]*m[3] + v[1]*m[7] + v[2]*m[11] + m[15];

	// convert homogenous to non homogenous coordinates

	ret[0] = ret[0] / w;
	ret[1] = ret[1] / w;
	ret[2] = ret[2] / w;
}

// WORKING
void Camera::translatef(const GLfloat x, const GLfloat y, const GLfloat z, GLfloat * matrixToBeTranslated) const
{
	GLfloat translationMatrix[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, x,y,z,1};

	matrix4by4TimesMatrix4by4(matrixToBeTranslated, translationMatrix, matrixToBeTranslated);

	//matrixToBeTranslated[12] = matrixToBeTranslated[0]*x + matrixToBeTranslated[4]*y + matrixToBeTranslated[8]*z + matrixToBeTranslated[12];
	//matrixToBeTranslated[13] = matrixToBeTranslated[1]*x + matrixToBeTranslated[5]*y + matrixToBeTranslated[9]*z + matrixToBeTranslated[13];
	//matrixToBeTranslated[14] = matrixToBeTranslated[2]*x + matrixToBeTranslated[6]*y + matrixToBeTranslated[10]*z + matrixToBeTranslated[14];
	//matrixToBeTranslated[15] = matrixToBeTranslated[3]*x + matrixToBeTranslated[7]*y + matrixToBeTranslated[11]*z + matrixToBeTranslated[15];
}

// WORKING
void Camera::loadIdentity(GLfloat * matrixToLoadIdentityIn) const
{
	memcpy(matrixToLoadIdentityIn, im, 16*sizeof(GLfloat));
}

// WORKING
void Camera::setAxisFromEyeAndCenterPoint()
{
	//------------------
	*zaxis = *eyeP - *centerP;
	*zaxis = zaxis->normalize();
	//------------------
	//Side = forward x up
	*xaxis = worldYaxis.cross(*zaxis);
	*xaxis = xaxis->normalize();
	//------------------
	//Recompute up as: up = side x forward
	*yaxis = zaxis->cross(*xaxis);
}

// WORKING
void Camera::setAxisFromCameraMatrix()
{
	(*xaxis)[0] = cm[0];
	(*xaxis)[1] = cm[4];
	(*xaxis)[2] = cm[8];

	(*yaxis)[0] = cm[1];
	(*yaxis)[1] = cm[5];
	(*yaxis)[2] = cm[9];

	(*zaxis)[0] = cm[2];
	(*zaxis)[1] = cm[6];
	(*zaxis)[2] = cm[10];
}

void Camera::setEyeAndCenterPointFromCameraMatrix()
{
	(*eyeP)[0] = cm[12] / cm[15];
	(*eyeP)[1] = cm[13] / cm[15];
	(*eyeP)[2] = cm[14] / cm[15];

	*centerP = *eyeP + *zaxis;
}

// WOKING
void Camera::setCameraMatrix()
{
	GLfloat tm[16]; // temp matrix
	//------------------
	tm[0] = (*xaxis)[0];	tm[4] = (*xaxis)[1];	tm[8] = (*xaxis)[2];	tm[12] = -eyeP->dot(*xaxis);
	tm[1] = (*yaxis)[0];	tm[5] = (*yaxis)[1];	tm[9] = (*yaxis)[2];	tm[13] = -eyeP->dot(*yaxis);
	tm[2] = (*zaxis)[0];	tm[6] = (*zaxis)[1];	tm[10] = (*zaxis)[2];	tm[14] = -eyeP->dot(*zaxis);
	tm[3] = 0;			tm[7] = 0;			tm[11] = 0;			tm[15] = 1;
	//------------------
	loadIdentity(cm);
	matrix4by4TimesMatrix4by4(cm, tm, cm);
	//translatef(-eyex, -eyey, -eyez, cm);
}